#include "rviz_custom_plugins/viewmodels/point_model.h"
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <algorithm>
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include <QJsonObject>


using rviz_custom_plugins::utils::addMetadata;

using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;


namespace rviz_custom_plugins
{

namespace viewmodels
{


PointModel::PointModel(QObject *parent): QStandardItemModel(parent) {
    setColumnCount(3);
    setHorizontalHeaderLabels({ "x", "y", "z" });
}


Qt::ItemFlags PointModel::flags(const QModelIndex& index) const {
    // 第一列不可编辑
    // if (index.column() == 0) {
    //     return QStandardItemModel::flags(index) & ~Qt::ItemIsEditable;
    // }
    Qt::ItemFlags flags = QStandardItemModel::flags(index) | Qt::ItemIsSelectable | Qt::ItemIsEnabled;
    return flags & ~Qt::ItemIsEditable;
}



bool PointModel::insertItem(int row, InteractiveMarker& intMarker) {
    if (row > rowCount()) {
        return false;
    }
    auto it = markers.begin();
    std::advance(it, row);

    markers.insert(it, intMarker);

    QList<QStandardItem*> pointRow;
    // pointRow << new QStandardItem(QString::fromStdString(intMarker.name));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.x,'f', 2));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.y,'f', 2));
    pointRow << new QStandardItem(QString::number(intMarker.pose.position.z,'f', 2));
    insertRow(row, pointRow);

    handleItemsReorder(row);

    emit itemOrderUpdated(row, markers);
    return true;
}


void PointModel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback) {
    auto it = std::find_if(markers.begin(), markers.end(), [feedback](const InteractiveMarker& im) { return im.name==feedback->marker_name; });
    if (it != markers.end()) {
        it->pose = feedback->pose;
    }
    QString markerName = QString::fromStdString(feedback->marker_name);
    int row = getItemRow(markerName);
    if(row >= 0) {
        setData(index(row, 0), QString::number(feedback->pose.position.x,'f', 2), Qt::DisplayRole);
        setData(index(row, 1), QString::number(feedback->pose.position.y,'f', 2), Qt::DisplayRole);
        setData(index(row, 2), QString::number(feedback->pose.position.z,'f', 2), Qt::DisplayRole);
    }
}



void PointModel::removeItem(int row) {
    auto it = markers.begin();
    std::advance(it, row);
    markers.erase(it);

    removeRow(row);
    handleItemsReorder(row);

    emit itemOrderUpdated(row, markers);
}


InteractiveMarker& PointModel::getRowItem(int row) {
    auto it = markers.begin();
    std::advance(it, row);
    return *it;
}


int PointModel::getItemRow(QString& markerName) {
    auto it = std::find_if(markers.begin(), markers.end(), [markerName](const InteractiveMarker& im) { return im.name==markerName.toStdString(); });
    if (it != markers.end()) {
        return std::distance(markers.begin(), it);
    }
    return -1;
}


void PointModel::handleItemsReorder(int row)
{
    auto markerIt = markers.begin();
    std::advance(markerIt, row); // 移动到指定索引
    int tmpRow = row;
    for ( ;markerIt != markers.end(); ++markerIt, ++tmpRow) {
        // 重新设置marker的row
        // QJsonObject metadata;
        // metadata[ROW] = QString::number(tmpRow);
        // setMetadata(*markerIt, metadata);
        addMetadata(*markerIt, ROW, QString::number(tmpRow));
    }
}


std::list<InteractiveMarker>& PointModel::getMarkers()
{
    return markers;
}


}

}
